from launch import LaunchDescription
from launch_ros.actions import Node
from launch.actions import DeclareLaunchArgument
from launch.substitutions import LaunchConfiguration
from launch.actions import IncludeLaunchDescription
from launch.launch_description_sources import PythonLaunchDescriptionSource
from ament_index_python.packages import get_package_share_directory
import os
from launch.conditions import IfCondition

def generate_launch_description():
    b2_description_pkg = get_package_share_directory("b2_description")
    b2_description_launch_path = os.path.join(b2_description_pkg, "launch", "display.launch.py")
    b2_driver_pkg = get_package_share_directory("b2_driver")
    rviz_cfg_path = os.path.join(b2_driver_pkg, "rviz", "display.rviz")
    parms_path = os.path.join(b2_driver_pkg, "params", "driver.yaml")

    declare_use_sim_time_argument = DeclareLaunchArgument(
        'use_sim_time',
        default_value='false',
        description='Use simulation (Gazebo) clock if true')
    
    use_sim_time = LaunchConfiguration('use_sim_time')

    use_rviz = DeclareLaunchArgument(
        name="use_rviz",
        default_value="false"
    )

    # 1.机器人模型可视化
    action_b2_description_launch = IncludeLaunchDescription(
        launch_description_source=PythonLaunchDescriptionSource(
            launch_file_path=b2_description_launch_path
        ),
        launch_arguments=[
            ("use_joint_state_publisher", "true"),
            ('use_sim_time', use_sim_time) 
        ]
    )

    node_params = [{'use_sim_time': use_sim_time}]

    ## 包含rviz2
    action_rviz2 = Node(
        package="rviz2",
        executable="rviz2",
        arguments=["-d", rviz_cfg_path],
        condition=IfCondition(LaunchConfiguration("use_rviz")),
        parameters=node_params
    )

    ## 雷达坐标系映射
    action_lidar_shine_upon = Node(
        package="tf2_ros",
        executable="static_transform_publisher",
        arguments=["--frame-id", "lidar_link", "--child-frame-id", "utlidar_lidar"],
        parameters=node_params
    )

    # 2.速度消息桥接
    action_twist_bridge = Node(
        package="b2_twist_bridge",
        executable="twist_bridge",
        parameters=node_params
    )
    
    action_driver = Node(
        package="b2_driver",
        executable="driver",
        parameters=[parms_path, {'use_sim_time': use_sim_time}]
    )

    imu_alias_node = Node(
        package="tf2_ros",
        executable="static_transform_publisher",
        arguments=["0", "0", "0", "0", "0", "0", "imu_link", "dog_imu_link"],
        parameters=node_params
    )

    lidar_alias_node = Node(
        package="tf2_ros",
        executable="static_transform_publisher",
        arguments=["0", "0", "0", "0", "0", "0", "lidar_link", "rslidar"],
        parameters=node_params
    )

    return LaunchDescription([
        declare_use_sim_time_argument,
        use_rviz,
        action_b2_description_launch,
        imu_alias_node,
        lidar_alias_node,
        action_rviz2,
        action_lidar_shine_upon,
        action_twist_bridge,
        action_driver
    ])